#include "MiniObject.h"

using namespace XYF;

MiniObject::~MiniObject()
{
    // do nothing
}

int XYF::FindSpan(Real u, int p, const MiniArray<Real> &knots)
{
    // 防止越界
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    // 返回左闭右开区间 u \in [u_i, u_{i+1})
    int m = knots.size() - 1;
    int n = m - p - 1;

    // 边界情况：如果 u 在区间最大值的边界上，就返回左边的区间
    // 注意这里边界是最大的值，而不是节点最大值
    if (u == knots[knots.size() - p - 1])
        return n;

    // 二分法寻找区间
    int low = p;
    int high = n + 1;
    int mid = (low + high) / 2;
    while (u < knots[mid] || u >= knots[mid + 1])
    {
        if (u < knots[mid])
            high = mid;
        else
            low = mid;
        mid = (low + high) / 2;
    }
    return mid;
}

int XYF::FindSpanMult(int k, Real u, int p, const MiniArray<Real> &knots)
{
    int n = 0;
    while (k >= p && knots[k] == u)
    {
        n++;
        k--;
    }
    return n;
}

MiniArray<Real> XYF::BasisFuncs(int i, Real u, int p, const MiniArray<Real> &knots)
{
    // 防止越界，确保索引在有效区间内
    assert(i >= p);
    assert(i < int(knots.size() - p - 1));
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    MiniArray<Real> N(p + 1);     // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    MiniArray<Real> left(p + 1);  // left[j] = u - u_{i+1-j}
    MiniArray<Real> right(p + 1); // right[j] = u_{i+j} - u

    N[0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        Real saved = 0;
        for (int r = 0; r < j; r++)
        {
            Real tmp = N[r] / (right[r + 1] + left[j - r]);
            N[r] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        N[j] = saved;
    }
    return N;
}

MiniArray2D<Real> XYF::DerivBasisFuncs(int d, int i, Real u, int p, const MiniArray<Real> &knots)
{
    // 排除无效参数
    assert(d <= p);
    assert(i >= p);
    assert(i < int(knots.size() - p - 1));
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    // ders[k][j] 表示 N_{i-p+j}^(k)
    MiniArray2D<Real> ders(d + 1, MiniArray<Real>(p + 1));

    // 先计算出非零基函数及节点差分
    MiniArray2D<Real> ndu(p + 1, MiniArray<Real>(p + 1));
    MiniArray<Real> left(p + 1);  // left[j] = u - u_{i+1-j}
    MiniArray<Real> right(p + 1); // right[j] = u_{i+j} - u

    ndu[0][0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        Real saved = 0;
        for (int r = 0; r < j; r++)
        {
            // 下三角部分存放节点差分
            // ndu[j][r] = u_{i + r + 1} - u_{i + r + 1 - j}
            ndu[j][r] = right[r + 1] + left[j - r];

            // 上三角部分（包括对角元）存放所有非零不大于 p 次基函数（AllBasisFuncs）
            // 注意这里 ndu[r][j] = N_{r,j}，没有像 AllBasisFuncs 一样反过来，因为空间已经分配好
            Real tmp = ndu[r][j - 1] / ndu[j][r];
            ndu[r][j] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        ndu[j][j] = saved;
    }

    // 将计算结果中的基函数部分（上三角）记录下来
    for (int j = 0; j < p + 1; j++)
        ders[0][j] = ndu[j][p];

    // N_{i,p}^(k) = p * [ N_{i,p-1}^(k-1) / (u_{i+p} - u_i) - N_{i+1,p-1}^(k-1) / (u_{i+p+1}-u_{i+1}) ]
    MiniArray2D<Real> a(2, MiniArray<Real>(p + 1));
    for (int r = 0; r < p + 1; r++)
    {
        // 计算 N_{i,r}^(k)
        Real s1 = 0;
        Real s2 = 1;

        // a_{0,0} = 1
        a[0][0] = 1;
        for (int k = 1; k < d + 1; k++)
        {
            Real der = 0;

            // a_{k,0} = a_{k-1,0} / (u_{i+p-k+1} - u_i)
            // 从 N_{i-p+r,p}^(k) 开始计算，这里 "i" 是 i-p+r
            // 所以取 ndu[p - k + 1][r - k] = u_{i+r-k+1} - u_{i-p+r+j}
            if (r >= k)
            {
                // a[s2] 存放 a_{k, }, a[s1] 存放 a_{k-1, }
                a[s2][0] = a[s1][0] / ndu[p - k + 1][r - k];
                der = a[s2][0] * ndu[r - k][p - k];
            }

            // 正常情况下 j1 = 1, j2 = k - 1，就是完整的计算求和
            // 特殊情况下要排除掉系数 a 为零和基函数 N 为零的情况
            int j1, j2;
            if (r - k >= -1)
                j1 = 1;
            else
                j1 = -r + k;

            if (r - 1 <= p - k)
                j2 = k - 1;
            else
                j2 = p - r;

            // a_{k,j} = (a_{k-1,j} - a_{k-1,j-1}) / (u_{i+p+j-k+1} - u_{i+j})
            // 注意这里 "i" 是 i-p+r
            // j = 1, ..., k-1
            for (int j = j1; j < j2 + 1; j++)
            {
                a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[p - k + 1][r - k + j];
                der += a[s2][j] * ndu[r - k + j][p - k];
            }

            // 计算对角 a_{k,k} = -a_{k-1,k-1} / (u_{i+p+1} - u_{i+k})
            // 注意这里 "i" 是 i-p+r
            if (r <= p - k)
            {
                a[s2][k] = -a[s1][k - 1] / ndu[p - k + 1][r];
                der += a[s2][k] * ndu[r][p - k];
            }
            ders[k][r] = der;

            // 交换行，将之前计算的 a_{k, } 作为下一个循环的 a_{k-1, }
            int s = s1;
            s1 = s2;
            s2 = s;
        }
    }

    // 由于重复求导，乘上因子 p * (p - 1) * ... * (p - k + 1)
    int r = p;
    for (int k = 1; k < d + 1; k++)
    {
        for (int j = 0; j < p + 1; j++)
            ders[k][j] *= r;
        r *= p - k;
    }
    return ders;
}

MiniArray2D<Real> XYF::AllBasisFuncs(int i, Real u, int p, const MiniArray<Real> &knots)
{
    assert(i >= p);
    assert(i < int(knots.size() - p - 1));
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    MiniArray2D<Real> N(p + 1);   // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    MiniArray<Real> left(p + 1);  // left[j] = u - u_{i+1-j}
    MiniArray<Real> right(p + 1); // right[j] = u_{i+j} - u

    N[0].push_back(1);
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        // N[j][r] = N_{r,j}，即 j 次 r 下标
        // 这里之所以反过来，是为了方便 push_back
        Real saved = 0;
        for (int r = 0; r < j; r++)
        {
            Real tmp = N[j - 1][r] / (right[r + 1] + left[j - r]);
            N[j].push_back(saved + right[r + 1] * tmp);
            saved = left[j - r] * tmp;
        }
        N[j].push_back(saved);
    }
    return N;
}

MiniArray<Real> XYF::Normalized(const MiniArray<Real> &t)
{
    MiniArray<Real> u = t;

    int m = u.size() - 1;
    Real umin = u[0];
    Real umax = u[0];
    for (int i = 0; i < m + 1; i++)
    {
        if (umin > u[i])
            umin = u[i];
        if (umax < u[i])
            umax = u[i];
    }

    Real du = umax - umin;
    for (int i = 0; i < m + 1; i++)
        u[i] = (u[i] - umin) / du;
    return u;
}

MiniArray<Vector3f> XYF::OrientBoundingBox(const MiniArray<Vector3f> &points)
{
    int n = points.size();

    // 坐标分离为 3 个随机变量
    Vector X(n), Y(n), Z(n);
    for (int i = 0; i < n; i++)
    {
        X[i] = points[i].x();
        Y[i] = points[i].y();
        Z[i] = points[i].z();
    }

    // 计算期望点
    Real E[] = {0, 0, 0};
    E[0] = X.sum() / n;
    E[1] = Y.sum() / n;
    E[2] = Z.sum() / n;

    // 计算协方差矩阵
    Matrix B(3, 3);
    for (int i = 0; i < n; i++)
    {
        B(0, 0) += X[i] * X[i];
        B(0, 1) += X[i] * Y[i];
        B(0, 2) += X[i] * Z[i];

        B(1, 0) += Y[i] * X[i];
        B(1, 1) += Y[i] * Y[i];
        B(1, 2) += Y[i] * Z[i];

        B(2, 0) += Z[i] * X[i];
        B(2, 1) += Z[i] * Y[i];
        B(2, 2) += Z[i] * Z[i];
    }
    B = B / n;

    // 计算 Xi_i 和 Xi_j 的协方差
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            B(i, j) -= E[i] * E[j];

    // 特征分解
    LinearSolver solver;
    Matrix V = solver.passJacobi(B, n * 10);

    // 比较对角元，进行排序
    int order[] = {0, 1, 2};
    for (int i = 0; i < 3; i++)
    {
        for (int j = 1; j < 3; j++)
        {
            // 比较相邻的元素，如果后者大于前者就交换
            if (B(j, j) > B(j - 1, j - 1))
            {
                int tmp = order[j - 1];
                order[j - 1] = order[j];
                order[j] = tmp;

                Real lambda = B(j - 1, j - 1);
                B(j - 1, j - 1) = B(j, j);
                B(j, j) = lambda;
            }
        }
    }

    // 三个主方向，按照从大到小的顺序
    Vector3f x(V(0, order[0]), V(1, order[0]), V(2, order[0]));
    Vector3f y(V(0, order[1]), V(1, order[1]), V(2, order[1]));
    Vector3f z(V(0, order[2]), V(1, order[2]), V(2, order[2]));
    x = normalize(x);
    y = normalize(y);
    z = normalize(z);

    // 计算在主方向上的投影长度，需要将期望平移到原点
    Vector3f Ev(E[0], E[1], E[2]);
    Matrix M(3, 2);
    for (int i = 0; i < n; i++)
    {
        Vector3f v = points[i] - Ev;
        Real p[] = {dot(x, v), dot(y, v), dot(z, v)};
        for (int j = 0; j < 3; j++)
        {
            if (M(j, 0) > p[j])
                M(j, 0) = p[j];
            if (M(j, 1) < p[j])
                M(j, 1) = p[j];
        }
    }

    // 记录在 x,y,z 正反方向的 6 个向量，以及中心向量 Ev
    MiniArray<Vector3f> box;
    for (int j = 0; j < 2; j++)
        box.push_back(x * M(0, j));
    for (int j = 0; j < 2; j++)
        box.push_back(y * M(1, j));
    for (int j = 0; j < 2; j++)
        box.push_back(z * M(2, j));
    box.push_back(Ev);

    // 调整法向方向
    if (dot(box[4], Vector3f{0, 1, 0}) < 0)
        for (int i = 0; i < 5; i++)
            box[i] = box[i] * -1;

    // 调整为右手坐标系，两个方向的先后顺序会决定光照时的法向
    if (dot(cross(box[0], box[2]), box[4]) < 0)
    {
        Vector3f tx = box[0];
        box[0] = box[2];
        box[2] = tx;

        // 注意正反方向的都要交换
        tx = box[1];
        box[1] = box[3];
        box[3] = tx;
    }

    return box;
}
